#include "MeasurE452AlgHighSs3ToSs7Circle.h"

MeasurE452AlgHighSs3ToSs7Circle::MeasurE452AlgHighSs3ToSs7Circle(int i):MeasurE452AlgBase(i)
{

}

void MeasurE452AlgHighSs3ToSs7Circle::operator()(cv::Mat &srcImg,
                                                 cv::Mat &srcImgback,
                                                 int id, std::map<string, float> &measRes,
                                                 int fixtureId)
{
    qDebug() << "start to E452 alg high ss3 to ss7 circle";

    std::vector<std::vector<float>> resptVec;
    resptVec.resize(5);
    for(auto i=35;i<40;i++)
    {
        resptVec[i-35].resize(3);
        std::vector<int> tparas;
        for(auto j=0;j< m_dataAllVec[i].m_edgeParam.cols;j++)
        {
            tparas.push_back(m_dataAllVec[i].m_edgeParam.at<int>(i-35,j));
        }
        //paras.push_back(tparas);
        std::vector<cv::Rect> rangVec;
        if(srcImg.empty()) {
            Q_ASSERT(0);
        }

        TemplateMatch(srcImg,m_dataAllVec[i],rangVec);
        cv::Mat imgt=srcImg(rangVec[0]).clone();
        std::vector<float> contours;
        MeasurE452EdgeDetect obj;
        obj.SmallCircle(imgt,tparas,contours,1);
        std::vector<cv::Point2f> ptsInVec={cv::Point2f(contours[0],contours[1])};
        ImageCoordinateTransformation(ptsInVec,rangVec);
        resptVec[i-35][2]=contours[2];
        std::unique_lock<std::mutex> lock(*MeasurE452AlgBase::mtxptrVec[id]);
        while (!MeasurE452AlgBase::flagVec[id])
        {
            (*MeasurE452AlgBase::cvarptrVec[id]).wait(lock);
        }
        //for(auto &elem:contours)
        {
            cv::Point2f ptsIn(ptsInVec[0].x,ptsInVec[0].y),ptsOut;
            CoordinateTransformation(datumBackVec[id],ptsIn,ptsOut);
            resptVec[i-35][0]=ptsOut.x;
            resptVec[i-35][1]=ptsOut.y;
        }
    }

    std::vector<std::vector<cv::Point>> resptVecback;
    resptVecback.resize(1);
    for(auto i=20;i<21;i++)
    {
        std::vector<int> tparas;
        for(auto j=0;j< m_dataAllVec[i].m_edgeParam.cols;j++)
        {
            tparas.push_back(m_dataAllVec[i].m_edgeParam.at<int>(i-20,j));
        }
        //paras.push_back(tparas);
        std::vector<cv::Rect> rangVec;

        if(srcImgback.empty()) {
            Q_ASSERT(0);
        }

        TemplateMatch(srcImgback,m_dataAllVec[i],rangVec);
        cv::Mat imgt=srcImgback(rangVec[0]).clone();
        std::vector<cv::Point> contours;
        MeasurE452EdgeDetect obj;
        obj.NewCircleEdge(imgt,tparas,contours);
        ImageCoordinateTransformation(contours,rangVec);

        std::unique_lock<std::mutex> lock(*MeasurE452AlgBase::mtxptrVec[id]);
        while (!MeasurE452AlgBase::flagVec[id])
        {
            (*MeasurE452AlgBase::cvarptrVec[id]).wait(lock);
        }
        for(auto &elem:contours)
        {
            cv::Point2f ptsIn(static_cast<float>(elem.x),static_cast<float>(elem.y)),ptsOut;
            CoordinateTransformation(datumBackVec[id],ptsIn,ptsOut);
            resptVecback[i-20].push_back(ptsOut);
        }
    }

    // go on coding
    //cout<<resptVec[0][0]<<"\t"<<resptVec[0][1]<<endl;
    MeasurE452EdgeDetect obj;
    vector<float> ss1res=obj.FitCircle(resptVecback[0]);
    measRes.insert(std::make_pair(string("FAI115-SS3-X-distance_pixel"), -resptVec[0][0]));
    float fai115x = CalcMeasureDistance(m_phypixpara[fixtureId][std::string("FAI115-SS3-X-distance")], -resptVec[0][0]);
    measRes.insert(std::make_pair(string("FAI115-SS3-X-distance_real"), fai115x));

    measRes.insert(std::make_pair(string("FAI116-SS4-X-distance_pixel"), -resptVec[1][0]));
    float fai116x = CalcMeasureDistance(m_phypixpara[fixtureId][std::string("FAI116-SS4-X-distance")], -resptVec[1][0]);
    measRes.insert(std::make_pair(string("FAI116-SS4-X-distance_real"), fai116x));

    measRes.insert(std::make_pair(string("FAI117-SS5-X-distance_pixel"), -resptVec[2][0]));
    float fai117x = CalcMeasureDistance(m_phypixpara[fixtureId][std::string("FAI117-SS5-X-distance")], -resptVec[2][0]);
    measRes.insert(std::make_pair(string("FAI117-SS5-X-distance_real"), fai117x));

    measRes.insert(std::make_pair(string("FAI118-SS6-X-distance_pixel"), -resptVec[3][0]));
    float fai118x = CalcMeasureDistance(m_phypixpara[fixtureId][std::string("FAI118-SS6-X-distance")], -resptVec[3][0]);
    measRes.insert(std::make_pair(string("FAI118-SS6-X-distance_real"), fai118x));

    measRes.insert(std::make_pair(string("FAI119-SS7-X-distance_pixel"), -resptVec[4][0]));
    float fai119x = CalcMeasureDistance(m_phypixpara[fixtureId][std::string("FAI119-SS7-X-distance")], -resptVec[4][0]);
    measRes.insert(std::make_pair(string("FAI119-SS7-X-distance_real"), fai119x));

    measRes.insert(std::make_pair(string("FAI122-SS3-Y-distance_pixel"), -resptVec[0][1]));
    float fai122y = CalcMeasureDistance(m_phypixpara[fixtureId][std::string("FAI122-SS3-Y-distance")], -resptVec[0][1]);
    measRes.insert(std::make_pair(string("FAI122-SS3-Y-distance_real"), fai122y));

    measRes.insert(std::make_pair(string("FAI123-SS4-Y-distance_pixel"), -resptVec[1][1]));
    float fai123y = CalcMeasureDistance(m_phypixpara[fixtureId][std::string("FAI123-SS4-Y-distance")], -resptVec[1][1]);
    measRes.insert(std::make_pair(string("FAI123-SS4-Y-distance_real"), fai123y));

    measRes.insert(std::make_pair(string("FAI124-SS5-Y-distance_pixel"), -resptVec[2][1]));
    float fai124y = CalcMeasureDistance(m_phypixpara[fixtureId][std::string("FAI124-SS5-Y-distance")], -resptVec[2][1]);
    measRes.insert(std::make_pair(string("FAI124-SS5-Y-distance_real"), fai124y));

    measRes.insert(std::make_pair(string("FAI124-SS6-Y-distance_pixel"), -resptVec[3][1]));
    float fai124y2 = CalcMeasureDistance(m_phypixpara[fixtureId][std::string("FAI124-SS6-Y-distance")], -resptVec[3][1]);
    measRes.insert(std::make_pair(string("FAI124-SS6-Y-distance_real"), fai124y2));

    measRes.insert(std::make_pair(string("FAI125-SS7-Y-distance_pixel"), -resptVec[4][1]));
    float fai125 = CalcMeasureDistance(m_phypixpara[fixtureId][std::string("FAI125-SS7-Y-distance")], -resptVec[4][1]);
    measRes.insert(std::make_pair(string("FAI125-SS7-Y-distance_real"), fai125));

    measRes.insert(std::make_pair(string("SS3-diam_pixel"), resptVec[0][2]*2));
    float faiSS3 = CalcMeasureDistance(m_phypixpara[fixtureId][std::string("SS3-diam")], resptVec[0][2]*2);
    measRes.insert(std::make_pair(string("SS3-diam_real"), faiSS3));

    measRes.insert(std::make_pair(string("SS4-diam_pixel"), resptVec[1][2]*2));
    float faiSS4 = CalcMeasureDistance(m_phypixpara[fixtureId][std::string("SS4-diam")], resptVec[1][2]*2);
    measRes.insert(std::make_pair(string("SS4-diam_real"), faiSS4));

    measRes.insert(std::make_pair(string("SS5-diam_pixel"), resptVec[2][2]*2));
    float faiSS5 = CalcMeasureDistance(m_phypixpara[fixtureId][std::string("SS5-diam")], resptVec[2][2]*2);
    measRes.insert(std::make_pair(string("SS5-diam_real"), faiSS5));

    measRes.insert(std::make_pair(string("SS6-diam_pixel"), resptVec[3][2]*2));
    float faiSS6 = CalcMeasureDistance(m_phypixpara[fixtureId][std::string("SS6-diam")], resptVec[3][2]*2);
    measRes.insert(std::make_pair(string("SS6-diam_real"), faiSS6));

    measRes.insert(std::make_pair(string("SS7-diam_pixel"), resptVec[4][2]*2));
    float faiSS7 = CalcMeasureDistance(m_phypixpara[fixtureId][std::string("SS7-diam")], resptVec[4][2]*2);
    measRes.insert(std::make_pair(string("SS7-diam_real"), faiSS7));

    measRes.insert(std::make_pair(string("FAI154-SS1SS4-X-distance_pixel"), -fabs(ss1res[1]-resptVec[1][1])));
    float fai154x = CalcMeasureDistance(m_phypixpara[fixtureId][std::string("FAI154-SS1SS4-X-distance")], -fabs(ss1res[1]-resptVec[1][1]));
    measRes.insert(std::make_pair(string("FAI154-SS1SS4-X-distance_real"), fai154x));

    measRes.insert(std::make_pair(string("FAI154-SS1SS4-Y-distance_pixel"), fabs(ss1res[0]-resptVec[1][0])));
    float fai154y = CalcMeasureDistance(m_phypixpara[fixtureId][std::string("FAI154-SS1SS4-Y-distance")], fabs(ss1res[0]-resptVec[1][0]));
    measRes.insert(std::make_pair(string("FAI154-SS1SS4-Y-distance_real"), fai154y));

//    measRes.insert(std::make_pair(string("FAI155-SS6SS4-X-distance_pixel"), -fabs(resptVec[3][0]-resptVec[1][0])));
//    float fai155x = CalcMeasureDistance(m_phypixpara[fixtureId][std::string("FAI155-SS6SS4-X-distance")], -fabs(resptVec[3][0]-resptVec[1][0]));
//    measRes.insert(std::make_pair(string("FAI155-SS6SS4-X-distance_real"), fai155x));

//    measRes.insert(std::make_pair(string("FAI155-SS6SS4-Y-distance_pixel"), -fabs(resptVec[3][1]-resptVec[1][1])));
//    float fai155y = CalcMeasureDistance(m_phypixpara[fixtureId][std::string("FAI155-SS6SS4-Y-distance")], -fabs(resptVec[3][1]-resptVec[1][1]));
//    measRes.insert(std::make_pair(string("FAI155-SS6SS4-Y-distance_real"), fai155y));

    measRes.insert(std::make_pair(string("FAI155-SS6SS4-X-distance_pixel"), -fabs(resptVec[3][1]-resptVec[1][1])));
    float fai155x = CalcMeasureDistance(m_phypixpara[fixtureId][std::string("FAI155-SS6SS4-X-distance")], -fabs(resptVec[3][1]-resptVec[1][1]));
    measRes.insert(std::make_pair(string("FAI155-SS6SS4-X-distance_real"), fai155x));

    measRes.insert(std::make_pair(string("FAI155-SS6SS4-Y-distance_pixel"), -fabs(resptVec[3][0]-resptVec[1][0])));
    float fai155y = CalcMeasureDistance(m_phypixpara[fixtureId][std::string("FAI155-SS6SS4-Y-distance")], -fabs(resptVec[3][0]-resptVec[1][0]));
    measRes.insert(std::make_pair(string("FAI155-SS6SS4-Y-distance_real"), fai155y));

    float fai154=obj.GetPosition(cv::Point2f(
                                     m_standardValMap["FAI154-SS1SS4-X-distance"],
                                     m_standardValMap["FAI154-SS1SS4-Y-distance"]),
                                 cv::Point2f(fai154x, fai154y));
    measRes.insert(std::make_pair(string("FAI154-SS1SS4-position"), fai154));

    float fai155 = obj.GetPosition(cv::Point2f(m_standardValMap["FAI155-SS6SS4-X-distance"],
                                               m_standardValMap["FAI155-SS6SS4-Y-distance"]),
                                   cv::Point2f(fai155x, fai155y));
    measRes.insert(std::make_pair(string("FAI155-SS6SS4-position"), fai155));

    qDebug() << "end to E452 alg high ss3 to ss7 circle";
}
